﻿ 

namespace Common.Algorithm.Core;

public class AStarService
{
    List<GraphEdge> _graphEdges = new List<GraphEdge>();

    /// <summary>
    /// 边给定的  ASTAR 算法
    /// </summary>
    /// <param name="stIndex"></param>
    /// <param name="edIndex"></param>
    /// <param name="edges">边</param>
    /// <returns></returns>
    public IList<GraphEdge> RunAlgm(int stIndex, int edIndex, IEnumerable<GraphEdge> edges)
    {
        PriorityQueue<int, double> queue =
            new PriorityQueue<int, double>(Comparer<double>.Create((a, b) => Math.Sign(a - b)));

        List<GraphEdge> graphEdges = new List<GraphEdge>();

        int count = edges.Count();
        int curIndex = stIndex;
        while (count > 0)
        {
            count--;
            foreach (GraphEdge edge in edges.AtSt(curIndex))
            {
                double fCost = edges.At(stIndex, curIndex).weight;
                double gCost = edges.At(curIndex, edIndex).weight;

                queue.Enqueue(edge.edVertexId, fCost + gCost);
            }

            int pervIndex = curIndex;
            curIndex = queue.Dequeue();
            graphEdges.Add(edges.At(pervIndex, curIndex));
            if (curIndex == edIndex) break;
        }
        _graphEdges = graphEdges;
        return graphEdges;
    }
    /// <summary>
    /// 获得给定边算法得到的路径节点
    /// </summary>
    /// <returns></returns>
    public IList<int> GetPathIds()
    {
        List<int> ids = _graphEdges.Select(e => e.stVertexId).ToList();
        ids.Add(_graphEdges.Last().edVertexId);
        return ids;
    }
     
    /// <summary>
    /// 自定义距离计算 ASTAR  算法  直接算出路径节点
    /// </summary>
    /// <param name="stIndex">起点</param>
    /// <param name="edIndex">终点</param>
    /// <param name="points">所有点</param>
    /// <param name="calcCostFunc">代价计算函数</param>
    /// <returns></returns>
    public IList<int> RunAlgm(int stIndex, int edIndex,
       List<int> points, Func<int, int, double> calcCostFunc)
    {
        List<int> pathPnts = new List<int>();

        PriorityQueue<int, double> queue =
            new PriorityQueue<int, double>(Comparer<double>.Create((a, b) => Math.Sign(a - b)));

        int curIndex = stIndex;
        pathPnts.Add(curIndex);

        int count = points.Count();
        while (count > 0)
        {
            count--;

            var nextIndexs = points
              .OrderBy(p => calcCostFunc(p, curIndex))
              .ToList()
              .GetRange(0, points.Count / 6);

            foreach (var nextIndex in nextIndexs)
            {
                double fCost = calcCostFunc(stIndex, nextIndex);
                double gCost = calcCostFunc(nextIndex, edIndex);

                queue.Enqueue(nextIndex, fCost + gCost);
            }

            curIndex = queue.Dequeue();
            pathPnts.Add(curIndex);

            if (curIndex == edIndex) break;
        }

        return pathPnts;
    }

}
